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13. abstract 


The equations of motion for the simulation of the Orbiter (Remote Manipulator 
System) Payload System assuming that the masses and inertias of the arm can be 
neglected are developed for implementation into the Space Vehicle Dynamics 
Simulation (SVDS) program. The arm flexibility is incorporated into the equations 
by the computation of flexibility terms for use in the joint servo model. The 
approach developed in this report is based on using the Jacobian transformation 
matrix to transform force and velocity terms between the configuration space and 
the task space to simplify the form of the equations. 
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RMS MASSLESS ARM DYNAMICS CAPABILITY IN THE SVDS 


1 . INTRODUCTION AND SUMMARY 

This report presents the formulation and equations for the dynamics to simulate 
the Remote Manipulator System (RMS) in the Space Vehicle Dynamics Simulation 
(SVDS). The equations presented are derived assuming the masses anci inertias 
of the RMS system are small relative to the mass and inertia of the orbiter 
and payload, and can be neglected. 

Section 2 presents a Newtonian formulation of the rigid body equation of motion 
of the Orbiter-RMS-Payload system. The massless arm assumption and the 
resolved rate law (ref. 1) are used in these equ^^cions. The interaction forces 
and moments at the tip of the end effector resulting from the output torques at 
each of the six joints are computed for the payload and the orbiter. These 
forces are inputs to the "equations of motion of the orbiter and for the payload 
The arm joint rates are computed from the translational and rotational veloci- 
ties of the two bodies. 

Section 3 contains a development of the equations required to compute the ami 
flexibility matrix, given the flexibility of each .nember. For this purpose, 
tlie arm has been divided into five beams. The first beam certains the longeron 
and the portion of the arm up to the shoulder pitch joint. The second beam is 
the upper arm, the third is the lower arm, the fourth is the wrist, and the 
fifth beam is the hand. From the arm flexibility matrix, a joint flexibility 
matrix is computed and used to compute six flexibility terms for inclusion in 
each of the six servos to simulate arm flexibility. 

Section 4 presents a set of formulas for the computation of static loads on 
the arm members of the joints. 

Section 5 contains a set of engineering flowcharts presenting the computations 
required to implement the capabilities described in sections 2-4 into the 
SVDS program. 


Appendix A, The RMS Jacobian Matrix, develops pelatfonshtps between veToelties 
and forces in a chain of rigid bodies and relates these relationships to the 
RMS Jacobian matrix. 

Appendix B, GaleuTatton of the Arm Flexibility Matrix, presents a set of 
equations that can be used to compute the flexibility matrix for a chain of 
beams, given the flexibility matrix for each beam. 

Appendix G develops equations for the kinetic energy and the virtual work for 
a chain of rigid bodies. These equations are used In conjunction with Lagrange's 
equation to write the equations of motion of a chain of rigid bodies. The 
masses and inertias of the inner bodies of the chain are assumed zero, and the 
resulting equations are related to the massTess arm equations presented In 
section 2. 

Because of the approximations made in the development of the equations of motion 
in this report, some conwents should be made about the method of verification 
used, and some general comments about when the validity of these equations will 
be questionable. The massless arm approach presented herein and subsequently 
being ineorporated Into the SVOS program requires extensive checkout to deter^ 
mine its level of validity. The primary checkout procedure used is to compare 
these results with eases run on the RMS Siimulation (RMSS) program. The standard 
types of checkout techniques for dynamics programs will be used; i.e., simple 
eases that have closed form solutions, balanee of energy and work, and 
engineering approximation checks. 

the limits of validity of the massless arm equations must be determined to a 
eertain extent by comparison with other programs. Some observation can be 
made, however. The lighter the payload, the worse the approxlimatlon, and it 
is felt that from a rigid body standpoint, about 1000 lb is the minimum payload 
weight that should be siimulated In the rigid body mode, the technique employed 
here for the incorporation of longeron and arm flexibility allows the simulation 
of the first six bending modes of the system. As the mass of the payload 
decreases, the separation between the rigid body bending modes (charaeterized 
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by end body motions) start to couple with the arm modes (characterized by 
small motion of the »nd bodies). How good the approach presented herein is 
can be determined by comparing the mode shapes and frequencies from a massless 
arm vibration analysis with a distributed mass arm vibration analysis. From 
a flexible arm standpoint, payloads of less than 10,000 lb would be suspect 
at the present time. 
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2. RIGID BODY EQUATIONS 


This section presents the massless arm RMS rigid body equations of motion for 
Implementation Into the SVOS program, using a Newtonian approach to the formula- 
tion of the equations. Appendix C presents a Lagranglan approach to the devel- 
opment of the equations of motion of a chain of rigid bodies; a Newtonian 
development for this type of system can be found in ref. 2. The assumption 
that the arm Is massless appears to be reasonable, based on a comparison of 
the mass of the orbiter and the mass of a prospective payloads relative to the 
mass of the RMS system itself. The orbiter weighs approximately 180,000 lb, 
the payloads weigh between 20,000 and 65,000 lb, and the movable portiofiS of 
the arm weigh approximately 500 lb. 


The following relationships can be written from reference 1. 


{t} = [J] 


T 



(2-1) 


IJ]{9> = 



( 2 - 2 ) 


If the arm is assumed to be massless, eq. (2-1) gives the output force and 
moment acting on the payload caused by the torque motors. An equal and 
opposite force and moment will act on the orbiter. 


Using eqs. (2-1) and (2-2) and the massless arm assumption, the end effector 
forces and moments can be written 




{t} 


The translational equations for body 1 (orbiter) can be written 


d^R, 


dt‘ 




(2-3) 


(2-4) 


4 



The rotational equations for body 1 are 

[A.jHa)^} + 1 lA^ ] {uj-j } • {M.|} - {Mg} - [£^]{Fg} 

The quaternion rates for body 1 are 






} [u)i ] 



(2-5) 


(2-6) 


The body to inertial transformation is computed 

[B,l = (% + Ze^tejl + Zlej)^) (2-7) 

The rigid body rotational and translational equations of motion for the 
second body are written 

" Ir * lB,HFg}) + (Sj) (2-8) 

CA2){(Ii2} + [H2)IA2 )(i*'2) = (” 2^ * ■®21^^”e' (£2) 

The relative rotational and translational velocity of the tip of the end 
effector with respect to the orbiter are computed 

T./(dR,| (dR, )\ 

(Vr) . - \^\j * - IB, 2) IE2H-2» 

( 2 - 11 ) 



Using eq. (2-2), the arm joint rates can be computed as 


(5) • U)"’ 



(2-12) 
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The following equations are used to compute the secondary oody to Inertial 

transformation matrix and the body 1 to body 2 transforma';1on matrix. 

[Bj] • (2-13) 

[Bj,] - (TpjHT, (6^)11X3(05)] + 63 + 94)HT3(e,)HTLBl (2-14) 

Eqs. (2-3) through (2-14) form a complete set of rigid b'jdy equations of 

motion for the Orbiter-RMS-Payload system, assuming that the arm Is massless. 

Definition of Symbols 

(t) 6x 1 matrix of the torques output from the joint servos 

[J] 6x6 Jacobian mairlx 

{Mg} 3x1 matrix of the components of the end effector moment vector in 
the orbiter body system 

{Fg} 3x1 matrix of the components of the end effector force vector In 
the orbiter body system 

{R^} 3x1 matrix of the components of the orbiter position vector in the 

Inertial system 

m, Mass of the orbiter 

I 

[B^l 3x3 transformation matrix from the orbiter body system to the 
Inertial system 

{F^ } 3x1 matrix of the external forces on the orbiter In the orbiter 

body system 

{g^ } 3x1 matrix of the components of the gravitational acceleration 

vector on the orbiter, In the Inertial system. 

[A| ] 3x3 Inertia matrix of t}ie orbiter 

3x1 matrix of the components of the orbiter angular velocity 
vector in the orbiter body system 

{M.J } 3x1 matrix of the moments of the external forces about the center 

of mass (CM) of the orbiter 
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{p^} 3x1 matrix of the position vector from the CM of the orbiter to the 

tip of the end effector in the orbiter body system 

e^ Scalar part of orbiter quaternion 

{eg} 3x1 matrix of the vector part of the orbiter quaternion 

{Rg} Position vector to CM of payload in the inertial system 

Mass of the payload 

[Bg] 3x3 transformation matrix from the payload body system to the 
Inertial system 

[Bg^] 3x3 transformation matrix from the orbiter system to the payload 

body system 

[B-|g] 3x3 transformation matrix from the payload body system to the 

orbiter body system 

{Pg} 3x1 position vector from the payload CM to the tip of the end 
effector in the payload body system 

{Vj^} 3x1 matrix of the relative translational velocity of the tip of 

the end effector in the orbiter body system 

3x1 matrix of the relative angular velocity of the tip of the end 
effector in the orbiter body system 

{0} 6x1 matrix of the joint rates 

[Tj^bI transformation matrix from the orbiter body system to the 

longeron system 

ITpgl 3x3 transformation matrix from the end effector system to the 
payload body system 
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3. ARM FLEXIBILITY 


This section presents the development of a technique for computing the terms 
needed to simulated massless arm flexibility, by c(xnput1ng the arm 
flexibility matrix and then generating the joint flexibility matrix. Joint 
flexibility terms are generated fr(xn the joint flexibility matrix and will 
be used in the joint servo model to simulate arm flexibility in the massless 
arm RMS simulat>»n in SVOS. The computation of the arm flexibility matrix 
is discussed in Appendix B. 

Two modes of arm flexibility simulation can be used, passive and active arm 
flexibility. In the passive arm case, the joints are considered locked and 
the neutral arm angles are used with current arm angles to compute joints 
torque from elastic rotations at the joints. In the active case, flexibility 
terms are generated and used in the joint servo model. 

From Appendix B, the flexibility matrix relates end effector elastic displace 
ments and rotations to forces and moments applied at the tip of the end 
effector. 

Using eqs. (2-1) and (2-2), the joint flexibility matrix is developed. 


[YHPg) = (6g) 

(3-1) 

IJl'^iPg) = -{Tp} 

(3-2) 

tJ]{60) => {6^} 

(3-3) 


Substituting eqs. (3-2) and (3-3) into eq. (3-1), the following formulas 
result. 


-1J1'’[y 1U1^ {Tp} = {60) 

-lYjHTp} » {60} 

where [yjI Is the joint flexibility matrix. 


(3-4) 

(3-5) 
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For the passive case, the joint torques resulting from arm flexibility are 
computed from eq. (3-5) as 

{Tp} ■ -lYj]'^{6e) (3-6) 

For the active case, the joint flexibility matrix is used to generate 
flexibility terms to use In the joint servo model. Presented below Is a 
technique that can be used to generate these terms. 

Let (y^jI be a 5x5 matrix derived from tYjl by deleting the 1th column and 
the ith row. 

Let be a 1x5 matrix derived from [yjI by taking the ith row and 

removing the ith elenent. 

The flexibility term for the 1th torque nwtor can be computed as 

^1 = (3-7) 
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4. STATIC LOADS IN THE ARN 


In this section, a set of equations to compute the static loads In the arm at 
each joint are presented. These loads are called static loads because they 
Ignore the Inertia of the arm links. Since the dynamics formulated in this 
report are based on a massless arm this type of loads model is completely 
consistent wl^h the dynamics. The equations presented in Appendix A of this 
report are completely applicable, in particular, equation (A-20) can be used. 
Noting that che end effector forces and moment are computed for the rigid 
body dynamics these quantities can be used to form the {P^} In eq. (A-20). 

The following set of equations result. 

<M,} . 1T2b](-{M^) - ((rjj) - ir^g)) « {F^)) 


* 1^361 

{V^} - ITjgHF^} 

«3> • ITjeK-We) - «--6B» - '’'3B» * 

(Vj) • -1T-,HF,) 

«4> • * '^'•6B> - ^'•aB'’ 

<»4> • IT4bHF^} 

{Mj} - 1T4 b 1(-{H^) - ({rgg! - {r^g}) x (F^)) 

(Vg) . -IT^gHF^} 


f«6> 

<^6> 


ITgBKtMg) * Urgg) - fr,g)) * (F^)) 

•T5B><%> 
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{Mg} = -tTggUM^} 
{Vg} = -[TegHF^) 

where 


are the moment and force acting on the shoulder in the shoulder 
yaw coordinate system 

{M2),{V2} are the moment and force acting at the shoulder on the upper arm 
in the upper ann coordinate system 

{MslJVs) are the moment and force acting at the elbow on the upper arm in 
the upper arm coordinate system 

are the moment and force acting at the elbow on the lower arm in 
the lower arm coordinate system 

{MglJVg} are the moment and force acting at the wrist pitch joint on the 
lower arm in the lower arm coordinate system 

{MglJVg) are the moment and force acting on the wrist at the wrist pitch 
joint in the wrist system 

are the moment and force acting on the wrist at the wrist yaw 
joint in the wrist system 

{Mg),{Vg} are the moment and force acting on the hand at the wrist yaw joint 
in the hand system 

{Mg},{Vg} are the moment and force acting on the hand at the end effector 
in the hand system 
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5. SVDS ROUTINES 


This section presents engineering flowcharts of the routines required to inter- 
face the massless arm dynamics described in the preceding sections with SVDS. 
Flowcharts also are presented for the static loads computations in section 4. 

Three basic routines are required to interface this capability. RHSFM computes 
the vehicle interaction forces and moments and the arm joint rates; RMSFLX 
computes the joint flexibility terms for input to the joint servo model; and 
RMLOAD computes the static loads on the arm members at the joints. 


12 


SUBROUTINE RMSFM 


Subroutine RMSFM 1s called from the Math Model Driver (MMD) each pass of the 
Integration and computes forces and moments on each body resulting from torque 
motor outputs. The joint angle rates are computed based on the translational 
and angular velocities of each of the bodies. 


^ START ^ 


i COMPUTE END EFFECTOR FC 

)RCES AND MOMENTS FROM 


ORQUI 

Wj)' 

: MOTOR OUTPUTS 

T -1 

= {t} 



i COMPUTE INTERACTION FORCES AND MOMENTS ON THE i 




{M,,l = -{«,} - 

{Fr 2> = tB2iHPe> 

{Mr 2> = * t£2]{FR2} 


COMPUTE RELATIVE VELOCITY AND RELATIVE 
ROTATIONAL VELOCITY OF END EFFECTOR 

{wr> * IB-|2Hu) 2} - {u)^} 





■ (ar) 




COMPUTE JOINT 
{§} = IJ]"^ 

RATE 

(^> 

- i 

] 



^ RETURN ^ 
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SUBROUTINE RMSFLX 


Subroutine RMSFLX Is called from the MMO at some Integer multiple of the 
Integration step size, just in front of the torque motor model. This routine 
computes flexibility terms for input to the torque motor model. 

Q smr ^ 


COMPUTE END EFFECTOR MASSLESS ARM FLEXIBILITY HgfRlT 


■VE-'t 

1«1 


1 , 10 ) 




ItTeiJ 101 


roi [Tg^i 


ly\y] rYjgi 
IyIjI tY22l 


llT^gJ [OJ 


101 (T^gl 


101 % 



A\ 

0 

0 


0 

0 

o" 


0 

0 

0~ 

1y!,I • 

0 

1 , 
Yj2 

0 

IrJjl ■ 

0 

0 

1 

^26 

‘"22^ ■ 

0 

^55 

0 


0 

0 

1 

^33 


0 

^35 

0 


0 

0 

^66_ 


COMPUTE JO F lEXieaiTY MATRIX 

-1 


[Yj] = 


COMPUTE FLEXBILITY TERMS FOR TORQUE MOTOR MODEL TO SIMULATE 
MASSLESS ARM FLEXIBILITY 

where 

tY^jl Is the S’^S matrix derived from [YjI by deleting the ith 
row and column 

7 \ T 

is the 5x1 matrix derived from [Yj] by taking the ith 
column and deleting the ith element 


RETURN 
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START 


COMPUTE END EFFECTOR MASSLESS ARM FLEXIBILITY MATRIX 


s 10] £Tg^J [0] Iy|iJ (yJzJ [0] % (r^g] 

- Z 


Iy 52 jJ[io] ct^bJIIio] % 


0 0 


0 0 


0 0 0 


!y),I • 0 Tjj 0 IyIj) • 0 0 Y« ‘I'm! • 0 yJs 0 


0 Y„ 0 


0 0 0 


0 0 


COMPUTE JOINT FLEXIBILITY MATRIX 


-1 T"^ 

iy^] " [J] IycHJ] 


COMPUTE FLEXBILITY TERMS FOR TORQUE MOTOR MODEL TO SIMULATE 


MASSLESS ARM FLEXIBILITY 

e ; 21J, 22,-1; 2K . , . 

= T'llj ■ ^ 1-1. — .6 

where 

op 

IyJP is the 5x5 matrix derived from [yjI by deleting the ith 
row and column 
PI T 

{y^j} is the 5x1 matrix derived from [Yj] by taking the ith 
column and deleting the ith element 



RETURN 







SUBROUTINE RMLOAD 


Subroutine RMLOAD computes the static forces and moments acting on the arm 
members at the joints and should be called In the output loop at some 
Integer multiple of the Integration step size. 


START ^ 

-jzirl 


COMPUTE FORCES AND MOMENTS AT THE JOINTS ON THE ARM MEMBERS 


{M,} 

{V,> 

W3} 

C»3> 

(V,} 

(Vj) 

(Hj) 

{My) 

(Vy) 

(Mg) 

(Vg) 

(Mg) 

(Vg) 


IT2 bH-{%) ■ 

-(TjgKF^) 

[T3,1({M.} ♦ 
tT 3 g 1 {-(M^) . 

-1^381 tv 

lT 4 gJ((M,) * 

fT^KF.) 

tT^gl(-(M,} - 
-(Tgg)(F,) 
(TggKCM^) ♦ 
(TsgKF,) 

n58i(-{%> - 

lTggl((M^} * 

-neBHM,) 




»^6B> 




(r^g)) X if^}) 




- f'^3B>J - 


({'■bb^ - 






(rgg)) X {F^» 


{r4B)) X (F^)) 


(r^g)) X (F,)) 






(rgg)) X {F^» 


RETURN ^ 
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APPENDIX A 


THE RMS JACOBIAN MATRIX 


The purpose of this appendix 1s to develop some kinematical relationships that 
deal with the computation of translational and rotational velocities In a 
chain of rigid bodies. These same kinematical relationships appear In relating 
forces and moments in the chain of bodies. As seen In reference 1, for a 
system of six joint degrees of freedom like the RMS, the so-called Jacobian 
matrix becomes an Invertible transformation relating configuration space 
velocity and force quantities to task space velocity and force quantities. 

These same kinematical relationships appear in the computation of the combined 
flexibility matrix for the system of rigid bodies. 

Suppose that we have a chain of rigid bodies with one degree of rotational 
freedom at each joint. (For more than one degree of freedom at a joint, a 
null vector can be assumed for the next position vector, thus putting both 
rotations at the same point). From figure A-1 we can write the following 
relationship 



A-1 



where Vq and Wq are the translational and rotational velocities of body zero 
(the first body of the chain). A recursive relationship can be written for 
the velocities of the 1th body at joint 1 In terms of the velocities In body 
1 - 1 at the 1 - 1 joint. 

V,- • 7,., + X 

® (A-2) 


Substituting eq. (A-2) into Itself, we arrive at a relationship for 7^ and 
In terms of 7 q, uJp, and the joint rates up to the 1th body. 



and 



(A-4) 


From eqs. (A-3) and (A-4) we can write 


Ck}> 

1 


■ "li 

[oT 


"*0 



i-1 

-E 

J =0 

"1 




"1 

“2 j... 

• • *0 


1-1 

1-1 ! 1 



■E " “1 

-E^j” “2 1 I ° 

0 

c. 

• 

j“i 

j=2 1 1 

1 1 


e 

w 



e 

-Q. 


(A-5) 


A-2 



If we define 


6x6 



(A-6) 


(A-7) 


(A-8) 


(A-9) 


Then eq. (A-S) can be written 


{V.} ■ iLn Hvj + [It, ] j a, ]••• 'i, (0)-”iomuHe) (a-io) 

1 0, 0 'l I M 



A-3 



(A-12) 


(V^) ■ [Lq HVg} + [s^huhS) 

For the RMS configuration we have seen that the relative velocity between the 
or biter and the payload can be written (ref. 1) as 

{V„} = (A-13) 

Relating eq. (A-13) to eq. (A-12), we see that 

[J] = (SgHU] (A-14) 


We have also seen that we can write the output forces and moments from the 
arm as 


{t} = IJ)’^ 



(A-I5) 


We will now take a closer look at the force moment kinematic relationships. 
Consider figure A-2. 




Figure A-2 



If vife consider this chain to be In static equilibrium, then we can write 


F 


1 



(A-16) 


n 

and **'1 ^ ^n 


We can write eqs. (A-16) and (A-17) as 


Defining 



and using definitions from above, we can write eq. (A-18) as 


(P,) = [L,„,il^{P„) 


(A-18) 


(A-19) 


(A-20) 


We have not discussed the fact that the various vectors given are In dif- 
ferent coordinate systems that change relative to each other as the bodies 
move and the angles change. We will now write these transformations between 
the coordinate systems fixed In each of the moving bodies. For simplicity 
without loss In generality. It will be assumed that all of the body fixed 
coordinate systems are aligned when all the angles are zero. 

We will now adopt the following notation: [B^j] denotes the transformation 

from the coordinate system fixed In body j to the coordinates system fixed 
In body 1. We will also note that 

^®1 1-1^ * ®i ^ (A-21) 


A-5 



and that 


'“ij’ * j] 1 > j (A-22) 

We win assume that the matrices defined In the first part of this appendix 
are written such that everything Is transformed to the 0 system unless other- 
wise specified. 
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APPENDIX B 


CALCULATION OF THE ARM FLEXIBILITY MATRIX 

The purpose of this appendix Is to present a set of formulas for computing 
the arm flexibility matrix or the flexibility matrix for a chain of bodies. 
By flexibility matrix Is meant the matrix that relates the forces applied 
to the end of a cantilevered chain of bodies to the resulting static-elastic 
displacement. For a given cantilevered body (fig. B-1), we can write 



Figure B-1 

Suppose the body depicted In figure B-1 Is the 1th body In a chain; If we 
apply a load to the end of the chain of bodies, we car compute the load at 
the 1th joint In terms of the end load. Assuming ■ IJ, from eg. (A-20) we 
have the load at the 1th hinge as 

{p,} • n-,„jAp„) (B-2) 


B-1 



The elastic deflection at the 1th joint can be written as 


(4,) • Iy,HP,} • (B-3) 

The resulting deflection at the end of the beam from an elastic deflection at 
the 1th joint assuming small angular deflection Is 

The total deflection at the end of the beam can now be written as the sum of 
the deflections expressed in eq. (B-4). 

n n 

- E = E ‘Sn' lLi„l^<P„) ' M {P„) (B-5) 

1-1 1-1 

So we see that 


[Yl 


n 


E 




1=1 


(B-6) 


We win assume that the [y^J matrices ir; the equations above are sparse 
matrices. Looking at figure B-2 we will assume that the [y^] matrices have 
the following form 


lYll 


yj^ 0 0 0 0 0 

0 Y 22 ® 0 0 yjg 

^ ° 1^33 ° ^35 ° 

000000 

° ° ^53 ° ^55 ° 

0 y^2 ° ° 0 Yjg 


(B-7) 


B-2 



Figure B-2 


It Is noted that [y^l is symmetric so there are only seven independent non- 
zero values in [y^]. All the elements of eq. (B-7) are positive numbers 
except for ygg and yg 2 » which are negative. 
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APPENDIX C 


LA6RAN6IAN APPROACH TO THE RIGID BODY EQUATIONS OF MOTION 
FOR A CHAIN OF RIGID BODIES 

The Rigid Body equations of motion for a chain of bodies will be developed In 
this appendix, using the relationships developed In Appendix A. After 
writing these equations In rather general terms, we will assume that we have 
six joints and that all of the Inner bodies are massless so that the approx- 
imations made In assuming that the arm Is massless can be noted. 

We will now look at the kinetic energy of the 1th body In a chain of rigid 
bodies (fig. C-1). 





C-I 


The kinetic energy of the 1th body written In terms of the velocities at the 
1th joint Is 

7 ^ • - S , X ( c - 2 ) 

= V, + «, X . 7, - J, X s, (C-3) 


Substituting eq. (C-3) Into eq. (C-1) 



0). 

1 

T 

1A,J - 1 






m.td,l 1 

m 


7i. 




(C-4) 


From eq. (C-4) we can write the kinetic energy of the entire chain of rigid 
body as 

n 

T = |■2{V^}^[A,l{V.} (C-5) 

1=0 

Using eq. (A-12) in eq. (C-5) we can write 


1=0 ' ’ 



’j[A,]/[LQ ]{Vq) + lS^HU]{5}j 


n “ 



^ (Lg 1^[A.JIS,J[U] 
1=0 ^ 


{Vq} 

n 



[U]^tS,)''’[A,l[S,HU] 


{6} 

1=0 
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From eq. (C-6) we can write 



T - j {ql^tAHq} 

(C-7) 

where 

{Vq) 

{q} • . 

LwJ 


(C-8) 


and in general tA] is a function of the q^'s. 

This completes writing expressions for the kinetic energy of the systan. As 
we are dealing with rigid bodies, we will assume that the strain energy of the 
system is zero. To complete the ingredients for Lagrange equations, we must 
formulate the virtual work of the system to define the generalized forces, 
the virtual work due to the torque motors at the joints can be written 

«Wj = {69)'^{t) = 6t{0}{T) (C-9) 


The virtual work of the external forces and moments applied to the ith body 
can be written as 

[k; 


n (M. ) n (H. 

i=0 1=0 ^''i 


(C-10) 


Using eq. (A-12) in eq. (C-10) we have 


6W = 6Wj + 6Wj, = 


n / \ ( ^i 

1.0 ' ^ ''fi' 


= («ro>'^e "-o/LI " " E 

1*0 ’ l' ' i=0 


(C-11) 
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or 


6W = {6q} 


fRj 


i*0 ’ 


n 


{t} If 

i=0 






(C-12) 


and since 6W = {6q}^{Q} (C-13) 

where (Q) are the generalized forces, we have conpleted the deve1o[»nent of the 
terms needed . 


Substituting eq. (C-5) into Lagrange's equations we have 

^([AHq}) - {{q}^IA]{q}) = {Q} 



(C-14) 


(C-15) 


(C-16) 
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where 




(0)''’ 

6+n 


• 

{0)''' 

(B) - 

j=7 

[%]■ 

fAiTBA 1 

^ L^J 

• 

• 



• 

1 

pe+nj 


Let us now go back to eq. (C-5) and assume that 

[A.] = 0 i = 1 ,5 n = 6 

then T = \ {Vq}''’[A(,]{Vq) + 1 {V 5)^[Ag){Vg} 


(C-17) 


(C-18) 


Also 


■{Vg} 

T 

■[Ag] t [Lg^l'^lAgHLg^l 

a HAgHSgHUl ■ 
6 


(7g) 

— 

CD» 

>-r^ 

1 


. [Ul^lSgl^tAgltL 1 

[Ul^[Sgr(Ag)(Sgl [U] 


{0}_ 
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Now from eq. (A-14) we see that 



{\l/' 

{ 0 } 


rCAg] t [L ] [Agl [L ] 

D D 


[JJ (Agl (L 1 
b 


[Lg I (Agl [jf 
6 

tJl^IAgKJ] 


(Vg) 

{ 0 } 


As we have seen for the arm, [J] is an invertible transformation (except for 
singularity points). So eq. (C-18) is a valid form of kinetic energy with a 
valid set of 12 generalized coordinates. If we assume that the external 
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forces and moments on the interior bodies are zero, then we need only to deter- 
mine the form to the contributions to the generalized forces from the torque 


motors . 

«Wj = {68 )^{t} = 6t{6}''^{T} 

(C-20) 

also 

[0H6) = {V^} = <Vg) - [LJIVq) 

(C-21) 

or 

{6} = [j]'’{Vg} - ui'’a(,„i{v„} 

(C-22) 






(C-23) 
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